﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace WpfApplication1.Modulators
{
    public class PIDBank
    {
        public double Kp { get; set; }
        public double Ki { get; set; }
        public double Kd { get; set; }
        public double Ta { get; set; } //sampling time
        public double e { get; set; } //error
        public double esum { get; set; } //error summ, integration
        public double eold { get; set; } //last error
        public double y { get; set; } //calculated value for aircraft aileron
        public double speed { get; set; }

        public double maxEsum { get; set; }

        public PIDBank()
        {
            esum = 0;
        }

        public double calculate()
        {
            errorSumLimiter();
            y = Kp * e + Ki * Ta * esum + Kd * (e - eold) / Ta;
            //y = Kp * e * (1 / Math.Pow(speed, 2)) + Ki * Ta * esum * (1 / Math.Pow(speed, 2)) + Kd * (e - eold) / Ta * (1 / Math.Pow(speed, 2));
            //Console.WriteLine(Kp+" * "+e+" + "+Ki+" * "+Ta+" * "+esum+" + "+Kd+" * ("+e+" - "+eold+") / "+Ta);
            //y = y * (1 / Math.Pow(speed, 2)) * 900;
            //yLimiter();
            eold = e;
            return y;
        }

        private void yLimiter()
        {
            if (y < -90)
            {
                y = -90;
            }
            else if (y > 90)
            {
                y = 90;
            }
        }

        private void errorSumLimiter()
        {
            esum += e;
            if (esum < maxEsum * -1)
            {
                esum = maxEsum * -1;
            }
            else if (esum > maxEsum * -1)
            {
                esum = maxEsum;
            }
        }
    }
}
